#ifndef Mcs_H_
#define Mcs_H_

#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <stdint.h>
# include <getopt.h>
# include <time.h>
# include <errno.h>
# include <iostream>

/*
    Bytes used to talk to the motor controller (dual set-up)
*/

#define INITIALPACKET 0xAA
#define MOTOR0FORWARDPACKET 0x08
#define MOTOR0FORWARDFASTPACKET 0x09
#define MOTOR0REVERSEPACKET 0x0A
#define MOTOR0REVERSEFASTPACKET 0x0B

#define MOTOR1FORWARDPACKET 0x0C
#define MOTOR1FORWARDFASTPACKET 0x0D
#define MOTOR1REVERSEPACKET 0x0E
#define MOTOR1REVERSEFASTPACKET 0x0F

#define MOTOR0COASTPACKET 0x06
#define MOTOR1COASTPACKET 0x07


/*
    Bytes used to talk to the motor controller (single set-up)
*/

/*#define INITIALPACKET 0xAA
#define MOTOR0FORWARDPACKET 0x88
#define MOTOR0FORWARDFASTPACKET 0x89
#define MOTOR0REVERSEPACKET 0x8A
#define MOTOR0REVERSEFASTPACKET 0x8B

#define MOTOR1FORWARDPACKET 0x8C
#define MOTOR1FORWARDFASTPACKET 0x8D
#define MOTOR1REVERSEPACKET 0x8E
#define MOTOR1REVERSEFASTPACKET 0x8F

#define MOTOR0COASTPACKET 0x86
#define MOTOR1COASTPACKET 0x87*/

#define FWVERSIONPACKET 0x81
#define ERRORPACKET 0x82
#define GETCONFIG 0x83
#define SETCONFIG 0x84

/*
    Bit location for the different errors
*/
#define DATAOVERRUNERRORBIT 3
#define FRAMEERRORBIT 4
#define CRCERRORBIT 5
#define FORMATERRORBIT 6
#define TIMEOUTERRORBIT 7

/*
    Constants for configuration
*/
#define CONFIG_DEVICEID 0
#define CONFIG_PWM 1
#define CONFIG_SHUTDOWN 2
#define CONFIG_TIMEOUT 3

/*
    PWM config values
    HF/LF = High/Low Frequency
    7/8 = 7/8 bits
*/
#define CONFIG_PWM_HF7 0
#define CONFIG_PWM_HF8 1
#define CONFIG_PWM_LF7 2
#define CONFIG_PWM_LF8 3
/*
  PWM constants defined by frequency
*/
#define CONFIG_PWM_31_5KHZ 0
#define CONFIG_PWM_15_7KHZ 1
#define CONFIG_PWM_7_8KHZ 2
#define CONFIG_PWM_3_9KHZ 3

#define CONFIG_OK 0
using namespace std;

class Mcs {
public:
//  Begin C2 Required Stuff-----------------------------------
    Mcs();
    //These functions control motion forwards and backwards
    void MoveForwardAtSpeed(uint8_t speed);
    void MoveBackwardAtSpeed(uint8_t speed);
    void Stop();
    //These functions turn the seeker left or right for a certain
    //number of seconds. Heavily Impacted by the Turning Speed
    int TurnLeft(uint8_t degrees);
    int TurnRight(uint8_t degrees);
    //This function adjusts seeker movement by x degrees
    //while in motion.
    void AdjustToStayOnLine(int degrees,int distOff);
//	End C2 Required Stuff-------------------------------------

    void begin();

    void motor0Forward(uint8_t speed);
    void motor1Forward(uint8_t speed);
    void motor2Forward(uint8_t speed);
    void motor3Forward(uint8_t speed);

    void motor0Reverse(uint8_t speed);
    void motor1Reverse(uint8_t speed);
    void motor2Reverse(uint8_t speed);
    void motor3Reverse(uint8_t speed);

    void motor0Coast();
    void motor1Coast();
    void motor2Coast();
    void motor3Coast();

    uint8_t getFirmwareVersion();
    uint8_t getError();

    bool hasDataOverrunError();
    bool hasDataOverrunError(bool fetchError);

    bool hasFrameError();
    bool hasFrameError(bool fetchError);

    bool hasCRCError();
    bool hasCRCError(bool fetchError);

    bool hasFormatError();
    bool hasFormatError(bool fetchError);

    bool hasTimeoutError();
    bool hasTimeoutError(bool fetchError);

    uint8_t getPWMParameter(int motor);
    bool setPWMParameter(uint8_t pwmParam);

    bool getShutdownOnError();
    bool setShutdownOnError(bool shutdown);

    uint8_t getSerialTimeout();
        void Error(const char *Msg);
        void Warning(const char *Msg);

    //Opens the Serial Port within the Mcs class because I can't get
    //SerialHelper to work
    int openSerialPort();
    void configMCs();

    //These functions allow the speed to be changed for making
    //a turn. The value should be percentage based. 0-100%
    void setTurningSpeed(int);
    int getTurningSpeed();

    //This methods should be given percentage values
    //Ex: Operate left motor forward at 80% speed = leftMotorForward(80);
    void leftMotorForward(int);
    void leftMotorReverse(int);
    void rightMotorForward(int);
    void rightMotorReverse(int);

    //These methods serve as the breaks for the seeker
    void Break();
    void stopMotor0();
    void stopMotor1();
    void stopMotor2();
    void stopMotor3();

    //Delay command
    void WaitSeconds(int seconds);

    void sendByte(uint8_t byte);
    void readByte();

    void setleftID(uint8_t);
    void setrightID(uint8_t);
    uint8_t getleftID();
    uint8_t getrightID();

private:
    int tty_fd;

    //Variables for wait command
    int diff;
    time_t rightnow;
    time_t start;

    //Motor Controller IDs
    uint8_t leftID;
    uint8_t rightID;

    //SerialHelper serialPort;
    int Turning_speed;
    int Back_turning_speed;
    int Turning_speed_45;

    //Current Motor Controller speeds
    uint8_t LeftFrontSpeed;
    uint8_t LeftRearSpeed;
    uint8_t RightFrontSpeed;
    uint8_t RightRearSpeed;

    int filePointer;

    uint8_t _resetPin;
    uint8_t _errByte;
    uint8_t _fwVersion;

    void MoveForward(uint8_t L0Speed, uint8_t L1Speed, uint8_t R0Speed, uint8_t R1Speed);
};

#endif
